#include "WPILib.h"
#include "Robot2489.h"

#define MOVEAWAYIMMEDIATELY 1
#define NEARZONE 2
#define MIDDLEZONE 3
#define FARZONE 4
#define NOTHINGTODO 5

/**
 * Let's assign someone to write code for this! (i.e. kick and then move
 * out of the way).
 */

void Robot2489::Autonomous()
{
//		int automode;
	GetWatchdog().SetEnabled(false);
	KarlKompressor->Start();

// Basically, just don't do anything for now.

/*	 if (!dseio->GetDigital(2))
	 {
	 	 automode=MOVEAWAYIMMEDIATELY;
	 }
	 if (!dseio->GetDigital(4))
	 {
	 	 automode=NEARZONE;
	 }
	 if (!dseio->GetDigital(6))
	 {
	 	 automode=MIDDLEZONE;
	 }
	 if (!dseio->GetDigital(8))
	 {
	 	 automode=FARZONE;
	 }
	 if (!dseio->GetDigital(1))
	 {
	 	 automode=NOTHINGTODO;
	 }
	 switch(automode)
	 {
		 case 1: //GTFO LOL
		 {
			 TurnRight();
			 break;
		 }
		 case 2: //near
		 {
			 RetractKicker();
			 Wait(KICKERRETRACTTIME);
			 ChargeKicker();
			 Wait(KICKERREADYINGTIME);
			 CloseLatch();
			 DriveForward(.6, FORWARDDRIVETIME);
			 kickFunction();
			 Wait(1);
			 TurnRight();
			 break;
		 }
		 case 3: //middle
		 {
			 Wait(9);
			 RetractKicker();
			 Wait(KICKERRETRACTTIME);
			 ChargeKicker();
			 Wait(KICKERREADYINGTIME);
			 CloseLatch();
			 DriveForward(.6, FORWARDDRIVETIME);
			 kickFunction();
			 Wait(1);
			 DriveForward(.6, FORWARDDRIVETIME);
			 kickFunction();
			 Wait(1);
			 TurnRight();
			 break;
		 }
		 case 4: //far
		 {
			 Wait(9);
			 RetractKicker();
			 Wait(KICKERRETRACTTIME);
			 ChargeKicker();
			 Wait(KICKERREADYINGTIME);
			 CloseLatch();
			 DriveForward(.6, FORWARDDRIVETIME);
			 kickFunction();
			 Wait(1);
			 DriveForward(.6, FORWARDDRIVETIME);
			 kickFunction();
			 Wait(1);
			 DriveForward(.6, FORWARDDRIVETIME);
			 kickFunction();
			 Wait(1);
			 TurnRight();
			 break;
		 }
		 case 5:
		 {
			 KarlKompressor->Start();
			 break;
		 }
	 }*/

}
